package de.hhn.mim.psb;

import static org.junit.Assert.assertTrue;
import static org.junit.Assert.assertNotNull;

import org.junit.Test;

public class FindTrafoTest {
	@Test
	public void findTrafoSimple(){		
		Vect3D a1  = new Vect3D(100,0,0); 
		Vect3D a2  = new Vect3D(0,100,0);		
		Vect3D a3  = new Vect3D(0,0,100);	

		Matrix3D result = Matrix3D.translation(new Vect3D());	

		//bisschen rotieren
		Vect3D[] rotated = buildRotatedVect(a1, a2, a3, -10,3, -42,0);
		Vect3D rot1 = rotated[0];
		Vect3D rot2 = rotated[1];
		Vect3D rot3 = rotated[2];
		//Trafo-Matrix berechnen
		result = LMRegist.calcTrafo(a1, a2, a3, rot1,rot2,rot3);
		//Testvektoren ausmultiplizieren
		Vect3D test1 = result.mult(rot1);
		Vect3D test2 = result.mult(rot2);
		Vect3D test3 = result.mult(rot3);

		assertTrue("Trafo-Matrix stimmt nicht!",a1.equals(test1));
		assertTrue("Trafo-Matrix stimmt nicht!",a2.equals(test2));
		assertTrue("Trafo-Matrix stimmt nicht!",a3.equals(test3));
	}
	
	/**
	 * erhaelt drei Punkte und drei Drehwinkel. Die Punkte werden um x-, y- und 
	 * z-Achse rotiert. Anschliessend werden die Original- und die transformierten 
	 * Punkte der Methode calcTrafo uebergeben und die (inverse) Transformation 
	 * berechnet, mit der die transformierten auf die Originalkoordinaten
	 * abgebildet werden. Wenn der Parameter print = true ist, werden die
	 * Transformationsmatrizen und das Ergebnis der Hin-Transformation ausgegeben.
	 *
	 */
	@Test
	public void findTrafoComplex(){
		Vect3D v1 = new Vect3D(100, 0, 0);
		Vect3D v2 = new Vect3D(0, 100, 0);
		Vect3D v3 = new Vect3D(0, 0, 100);
		for(int x = 0; x <= 181; x++){
			double rx = (double) x;
			for(int y = 0; y <= 181; y++){
				double ry = (double) y;
				for(int z = 0; z <= 181; z++){
					double  rz    = (double) z;
					int     print = 0;
					testCalcTrafo2(v1, v2, v3, rx, ry, rz, print);
				}         
			}
		}
	}
	
	public void testCalcTrafo2(Vect3D v1, Vect3D v2, Vect3D v3,
								double rx, double ry, double rz,
								int print){
		Vect3D [] fixed    = {v1,v2,v3};
		Vect3D [] floating = buildRotatedVect(v1, v2, v3, rx, ry, rz, print);	

		LMRegist.LMPair [] lms = new LMRegist.LMPair[3];
		for(int i = 0; i < 3; i++){
			lms[i] = new LMRegist.LMPair(fixed[i], floating[i]);
		}
		int myPrint = 0;
		Matrix3D resMat = LMRegist.calcTrafo(lms[0].first, lms[1].first, lms[2].first,
				lms[0].second, lms[1].second, lms[2].second,
				myPrint);
		
		assertNotNull("Result matrix war NULL bei: ("+rx+","+ry+","+rz+")",resMat);

		Vect3D r0 = new Vect3D();
		Vect3D r1 = new Vect3D();
		Vect3D r2 = new Vect3D();

		r0 = resMat.mult(lms[0].second);
		r1 = resMat.mult(lms[1].second);
		r2 = resMat.mult(lms[2].second);

		String errorMessage = "Test1 fehlgeschlagen bei ("+rx+","+ry+","+rz+")";
		assertTrue(errorMessage,r0.equals(lms[0].first));
		assertTrue(errorMessage,r1.equals(lms[1].first));
		assertTrue(errorMessage,r2.equals(lms[2].first));
	}
	
	private static Vect3D[] buildRotatedVect(Vect3D v1,Vect3D v2, Vect3D v3, 
			double rx,double ry,double rz,int print){
		Vect3D [] floating = new Vect3D [3];
		Vect3D [] fixed    = new Vect3D [3];

		fixed[0]    = v1;
		fixed[1]    = v2;
		fixed[2]    = v3;
		floating[0] = v1;
		floating[1] = v2;
		floating[2] = v3;

		Vect3D trans    = new Vect3D(0, 0, 0);
		Vect3D trans2   = new Vect3D(0, 0, 0);
		Matrix3D t1Mat  = Matrix3D.translation(trans);
		Matrix3D trafo1 = new Matrix3D(1);
		Matrix3D rx1Mat = Matrix3D.rotationX(Math.toRadians(rx));
		Matrix3D ry1Mat = Matrix3D.rotationY(Math.toRadians(ry));
		Matrix3D rz1Mat = Matrix3D.rotationZ(Math.toRadians(rz));

		trafo1 = t1Mat;
		Vect3D [] rTrans = new Vect3D[floating.length];
		if(print > 1) System.out.println("Translation");
		for(int i = 0; i < floating.length; i++)
		{
			rTrans[i] = trafo1.mult(floating[i]);
			if(print > 1) rTrans[i].print();
		}
		if(print > 1) System.out.println();
		trafo1 = rx1Mat;
		Vect3D [] rRx = new Vect3D[floating.length];
		if(print > 1) System.out.println("RotationX");
		for(int i = 0; i < floating.length; i++)
		{
			rRx[i] = trafo1.mult(floating[i]);
			if(print > 1) rRx[i].print();
		}
		if(print > 1) System.out.println();
		trafo1 = ry1Mat;
		Vect3D [] rRy = new Vect3D[floating.length];
		if(print > 1) System.out.println("RotationY");
		for(int i = 0; i < floating.length; i++)
		{
			rRy[i] = trafo1.mult(floating[i]);
			if(print > 1) rRy[i].print();
		}
		if(print > 1) System.out.println();
		trafo1 = rz1Mat;
		Vect3D [] rRz = new Vect3D[floating.length];
		if(print > 1) System.out.println("RotationZ");
		for(int i = 0; i < floating.length; i++)
		{
			rRz[i] = trafo1.mult(floating[i]);
			if(print > 1) rRz[i].print();
		}
		if(print > 1) System.out.println();

		trafo1 = rz1Mat.mult(t1Mat); 
		Vect3D [] rRzT = new Vect3D[floating.length];
		if(print > 1) System.out.println("RotationZ - Translation");
		for(int i = 0; i < floating.length; i++)
		{
			rRzT[i] = trafo1.mult(floating[i]);
			if(print > 1) rRzT[i].print();
		}

		trafo1.identity();
		trafo1 = trafo1.mult(t1Mat);
		if(print > 1) t1Mat.print("tMat");
		if(print > 1) trafo1.print("tMat * trafo");
		trafo1 = trafo1.mult(rx1Mat);
		if(print > 1) trafo1.print("rxMat * tMat * trafo");
		trafo1 = trafo1.mult(ry1Mat);
		if(print > 1) trafo1.print("ryMat * rxMat * tMat * trafo");    
		trafo1 = trafo1.mult(rz1Mat);
		if(print > 1) trafo1.print("rzMat * ryMat * rxMat * tMat * trafo");

		Vect3D [] result = new Vect3D[floating.length];
		if(print > 1) System.out.println("RotZ - RotY - RotX - Translation");

		Matrix3D trafo2 = new Matrix3D(1);
		Matrix3D t2Mat  = Matrix3D.translation(trans2);
		Matrix3D rx2Mat = Matrix3D.rotationX(Math.toRadians(-rx));
		Matrix3D ry2Mat = Matrix3D.rotationY(Math.toRadians(-ry));
		Matrix3D rz2Mat = Matrix3D.rotationZ(Math.toRadians(-rz));

		trafo2.identity();
		trafo2 = trafo2.mult(t2Mat);
		if(print > 1) t2Mat.print("t2Mat");
		if(print > 1) trafo2.print("t2Mat * trafo");
		trafo2 = trafo2.mult(rx2Mat);
		if(print > 1) trafo2.print("rxMat * t2Mat * trafo");
		trafo2 = trafo2.mult(ry2Mat);
		if(print > 1) trafo2.print("ryMat * rxMat * t2Mat * trafo");    
		trafo2 = trafo2.mult(rz2Mat);
		if(print > 1) trafo2.print("rzMat * ryMat * rxMat * t2Mat * trafo");
		for(int i = 0; i < fixed.length; i++)
		{
			result[i] = trafo2.mult(fixed[i]);
			if(print > 0) result[i].print("fixed[" + i + "]   ");
			fixed[i] = result[i];      
		}
		for(int i = 0; i < floating.length; i++)
		{
			result[i] = trafo1.mult(floating[i]);
			if(print > 0) result[i].print("floating[" + i + "]");
			floating[i] = result[i];      
		}
		return floating;
	}
}